/**
 * Copyright ? Goome Technologies Co., Ltd. 2009-2019. All rights reserved.
 * File name:        applied_math.c
 * Author:           王志华       
 * Version:          1.0
 * Date:             2019-02-28
 * Description:      应用数学运算
 * Others:      
 * Function List:    
    1. 获取数值的符号
    2. 四舍五入取整
    3. 三维向量取模
    4. 二维向量取模
    5. 三维向量的归一化处理
    6. 二维向量的归一化处理
    7. 三维向量内积（点积）
    8. 二维向量内积（点积）
    9. 三维向量叉积
    10.三维向量之间的夹角
    11.根据经纬度计算两点间距离
 * History: 
    1. Date:         2019-02-28
       Author:       王志华
       Modification: 创建初始版本
    2. Date: 		 
	   Author:		 
	   Modification: 

 */

#include "applied_math.h"
#include <math.h>
#include <stdlib.h>


static const float PI = 3.1415926535898;

//地球半径(公里)
static const float EARTH_RADIUS = 6378.137;


// CRC-ITU 查表算法
static const U16 common_crc16_tab[] =
{
    0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
    0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
    0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
    0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
    0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
    0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
    0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
    0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
    0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
    0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
    0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
    0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
    0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
    0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
    0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
    0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
    0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
    0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
    0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
    0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
    0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
    0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
    0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
    0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
    0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
    0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
    0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
    0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
    0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
    0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
    0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
    0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78,
};



static const U8 epo_crc16_tab_h[] = 
{
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};


static const U8 epo_crc16_tab_l[] = 
{
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 
    0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 
    0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 
    0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 
    0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 
    0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 
    0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 
    0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 
    0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 
    0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 
    0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 
    0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 
    0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 
    0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 
    0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 
}; 


char applied_math_get_sign(double value)
{
    return value < -1E-6 ? -1 : value > 1E-6;
}

S32 applied_math_round(double value)
{
    return (int)(applied_math_get_sign(value) * (fabs(value) + 0.50001));
}

double applied_math_get_magnitude_3d(Vector3D v)
{
    return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
}

double applied_math_get_magnitude_2d(Vector2D v)
{
    return sqrt(v.x * v.x + v.y * v.y);
}

Vector3D applied_math_normalize_3d(Vector3D v)
{
    Vector3D ret_aclr = {0, 0, 0};
    double mag = applied_math_get_magnitude_3d(v);
    ret_aclr.x = v.x / mag;
    ret_aclr.y = v.y / mag;
    ret_aclr.z = v.z / mag;
    return ret_aclr;
}

Vector2D applied_math_normalize_2d(Vector2D v)
{
    Vector2D ret_aclr = {0, 0};
    double mag = applied_math_get_magnitude_2d(v);
    ret_aclr.x = v.x / mag;
    ret_aclr.y = v.y / mag;
    return ret_aclr;
}

float applied_math_inner_product_3d(Vector3D a, Vector3D b)
{
    return (a.x * b.x + a.y * b.y + a.z * b.z);
}

float applied_math_inner_product_2d(Vector2D a, Vector2D b)
{
    return (a.x * b.x + a.y * b.y);
}

Vector3D applied_math_cross_product_3d(Vector3D u, Vector3D v)
{
    Vector3D ret_aclr = {0, 0, 0};
    ret_aclr.x = u.y * v.z - u.z * v.y;
    ret_aclr.y = u.z * v.x - u.x * v.z;
    ret_aclr.z = u.x * v.y - u.y * v.x;
    return ret_aclr;
}

U8 applied_math_get_angle_3d(Vector3D a, Vector3D b)
{
    float product = 0;
    float a_magnitude = 0;
    float b_magnitude = 0;
    float cosine = 0;
    float radian = 0;
    float angle = 0;

    product = applied_math_inner_product_3d(a, b);
    a_magnitude = applied_math_get_magnitude_3d(a);
    b_magnitude = applied_math_get_magnitude_3d(b);
    if (fabs(a_magnitude * b_magnitude) < 1E-8)
    {
        return 0;
    }
    cosine = product / (a_magnitude * b_magnitude);
    radian = acos(cosine);
    angle = 180 * radian / PI;

    return applied_math_round(angle);
}

U16 applied_math_get_angle_diff(U16 angle_degree1, U16 angle_degree2)
{
	U16 diff = abs((angle_degree1 + 360 - angle_degree2)%360);
	return (diff > 180) ? (180 - diff%180) : diff;
}

double applied_math_get_distance(double start_lng, double start_lat, double end_lng, double end_lat)
{
    double start_lat_rad = start_lat * PI / 180.0;
    double start_lng_rad = start_lng * PI / 180.0;
    double end_lat_rad = end_lat * PI / 180.0;
    double end_lng_rad = end_lng * PI / 180.0;
    double lat_diff = end_lat_rad - start_lat_rad;
    double lng_diff = end_lng_rad - start_lng_rad;
    double a = pow(sin(lat_diff / 2),2) + cos(start_lat_rad)*cos(end_lat_rad)*pow(sin(lng_diff / 2),2);
    double distance = 2 * asin(sqrt(a)) * EARTH_RADIUS * 1000;
    return distance;
}
 
U16 applied_math_calc_common_crc16(const U8 * p_data, U32 len)
{
    U16 crc = 0xFFFF;

	if (NULL == p_data || len > 0x48000)
	{
		return crc;
	}
    
    while(len > 0)
    {
        crc = (crc >> 8) ^ common_crc16_tab[(crc ^ (*p_data)) & 0xFF];
        p_data++;
        len--;
    }
    
    return ~crc;
}

U32 applied_math_calc_epo_crc16(const U8* p_data, U32 len, U32 pre_val)
{  
    U32 crc_h = (pre_val & 0xFF00) >> 8;
    U32 crc_l = pre_val & 0xFF00;
    U32 tab_index = 0;
    U32 i = 0;

	if (NULL == p_data || len > 0x28000)
	{
        return ((crc_h & 0x00FF) << 8) | (crc_l & 0x00FF) & 0xFFFF;
    }
    
    for (i = 0; i < len; ++i)
    {  
        tab_index = (crc_l ^ p_data[i]) & 0x00FF;  
        crc_l = crc_h ^ epo_crc16_tab_h[tab_index];  
        crc_h = epo_crc16_tab_l[tab_index];
    }
    
    return ((crc_h & 0x00FF) << 8) | (crc_l & 0x00FF) & 0xFFFF;
}

float applied_math_lowpass_filter(const float last_value, const float new_value, float alpha)
{
	return last_value * alpha + new_value * (1 - alpha);
}

float applied_math_avage(const float* p_array,const U16 len)
{
	U16 index = 0;
	float sum = 0;
	for(index = 0;index < len;index++)
	{
		sum += p_array[index];
	}
	return sum/len;
}

